#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include<tf/transform_datatypes.h>
#include<Eigen/Eigen>
#include<fstream>

bool is_get_tracker_first_gps_odom_ = false;
tf::StampedTransform tf_tracker_first_,tf_tracker_last_;
tf::Vector3 tracker_position_last_;

ros::Publisher tracker_odom_pub_,tracker_odom_path_pub_;
nav_msgs::Path tracker_odom_path_;
double tracker_odom_first_yaw_;

ros::Publisher slam_odom_pub_,slam_odom_path_pub_;
nav_msgs::Path slam_odom_path_;

void saveTrajectory(std::string& out_file,double time_stamp, double x,double y,double z,double ow,double ox,double oy,double oz, double vx,double vy,double vz)
{
    std::ofstream outfile(out_file.c_str(),std::ios::out|std::ios::binary|std::ios::app);
    if(!outfile) std::cout<<"error"<<std::endl;
    outfile<<std::to_string(time_stamp)<<" "<<std::to_string(x)<<" "<<std::to_string(y)<<" "<<std::to_string(z)<<" "    //xyz
           <<std::to_string(ow)<<" "<<std::to_string(ox)<<" "<<std::to_string(oy)<<" "<<std::to_string(oz)<<" "<<
           std::to_string(vx)<<" "<<std::to_string(vy)<<" "<<std::to_string(vz)<<std::endl;  //
    outfile.close();
}

void tracker_gps_odom_CB(const nav_msgs::OdometryConstPtr& tracker_gps_odom){
    if(!is_get_tracker_first_gps_odom_){
        tf::Transform tf_temp;
        tf::poseMsgToTF(tracker_gps_odom->pose.pose, tf_temp);
        tf_tracker_first_ = tf::StampedTransform(tf_temp, tracker_gps_odom->header.stamp, "gps", "tracker");
        tf_tracker_last_ = tf_tracker_first_;
        double roll_temp,pitch_temp;
        tf::Matrix3x3(tf_tracker_first_.getRotation()).getRPY(roll_temp,pitch_temp, tracker_odom_first_yaw_);
        tracker_odom_first_yaw_ = -tracker_odom_first_yaw_;
        is_get_tracker_first_gps_odom_ = true;
        ROS_INFO_STREAM("get first tracker gps odom: "<<tf_tracker_first_.stamp_.toSec()<<" "<<
            tf_tracker_first_.getOrigin().getX()<<" "<<tf_tracker_first_.getOrigin().getY()<<" "<<tf_tracker_first_.getOrigin().getZ()<<" "<<tracker_odom_first_yaw_);
        return;
    }
    tf::Transform tf_temp;
    tf::poseMsgToTF(tracker_gps_odom->pose.pose, tf_temp);
    tf::StampedTransform tf_tracker_cur(tf_temp, tracker_gps_odom->header.stamp, "gps", "tracker");

    // tf::Transform tf_delta = tf_tracker_first_.inverse()*tf_tracker_cur;
    // double roll_temp, yaw_temp,pitch_temp;
    // tf::Matrix3x3(tf_delta.getRotation()).getRPY(roll_temp, pitch_temp, yaw_temp);
    // // ROS_DEBUG_STREAM("r p y: "<<roll_temp<<" "<<pitch_temp<<" "<<yaw_temp);
    // tf_delta.setRotation(tf::Quaternion(0, 0,yaw_temp));
    // tf_delta.setOrigin(tf::Vector3(tf_delta.getOrigin().getX(), -tf_delta.getOrigin().getY(), 0));

    tf::Vector3 tracker_position(tf_tracker_cur.getOrigin().getX() - tf_tracker_first_.getOrigin().getX(), 
                        tf_tracker_cur.getOrigin().getY()-tf_tracker_first_.getOrigin().getY(), 0);
    tf::Transform tf_convert;
    tf_convert.setOrigin(tf::Vector3(0,0,0));
    tf_convert.setRotation(tf::Quaternion(0,0,tracker_odom_first_yaw_));
    tracker_position = tf_convert*tracker_position;
    tracker_position = tf::Vector3(tracker_position[0], -tracker_position[1], 0);
    tf_convert.setRotation(tf::Quaternion(0,0,-M_PI/2+0.03));
    tracker_position = tf_convert*tracker_position;
    tracker_position = tf::Vector3(tracker_position[0]+0.82, tracker_position[1], 0);


    tf::Transform tf_delta = tf_tracker_first_.inverse()*tf_tracker_cur;
    double roll_temp,pitch_temp,yaw_temp;
    tf::Matrix3x3(tf_delta.getRotation()).getRPY(roll_temp,pitch_temp,yaw_temp);
    tf_delta.setRotation(tf::Quaternion(0, 0, -yaw_temp));
    tf_delta.setOrigin(tracker_position);

    tf::Vector3 tracker_position_cur(tf_delta.getOrigin().getX(),tf_delta.getOrigin().getY(),tf_delta.getOrigin().getZ());
    tf::Vector3 velocity_world = (tracker_position_cur-tracker_position_last_)/(tf_tracker_cur.stamp_-tf_tracker_last_.stamp_).toSec();
    // ROS_DEBUG_STREAM("tracker velocity: "<<std::to_string(velocity_world[0])<<" "<<std::to_string(velocity_world[1])<<" "<<velocity_world[2]);
    // ROS_DEBUG_STREAM("tracker raw: "<<std::to_string(tracker_gps_odom->pose.pose.position.x)<<" "<<velocity_world[1]<<" "<<velocity_world[2]);
    tf_tracker_last_ = tf_tracker_cur;
    tracker_position_last_ = tracker_position_cur;

    //broadcast tf
    static tf::TransformBroadcaster broadcaster;//1.定义一个广播broadcaster
    broadcaster.sendTransform(tf::StampedTransform(tf_delta, ros::Time::now(), "world", "tracker"));

    //pub odom
    nav_msgs::Odometry tracker_odom_cur_msg;
    tracker_odom_cur_msg.header.frame_id = "world";
    tracker_odom_cur_msg.child_frame_id = "tracker";
    tracker_odom_cur_msg.header.stamp = tracker_gps_odom->header.stamp;
    geometry_msgs::Pose pose_msg_temp;
    tf::poseTFToMsg(tf_delta, pose_msg_temp);
    tracker_odom_cur_msg.pose.pose = pose_msg_temp;
    tracker_odom_cur_msg.twist.twist.linear.x = velocity_world[0];
    tracker_odom_cur_msg.twist.twist.linear.y = velocity_world[1];
    tracker_odom_cur_msg.twist.twist.linear.z = velocity_world[2];
    tracker_odom_pub_.publish(tracker_odom_cur_msg);

    //pub path
    geometry_msgs::PoseStamped pose_stamped_temp;           
    pose_stamped_temp.pose=pose_msg_temp;
    pose_stamped_temp.header.stamp= tracker_gps_odom->header.stamp;
    pose_stamped_temp.header.frame_id="world";
    tracker_odom_path_.header=pose_stamped_temp.header;
    tracker_odom_path_.poses.emplace_back(pose_stamped_temp);
    tracker_odom_path_pub_.publish(tracker_odom_path_);
}

void slam_odom_CB(const nav_msgs::OdometryConstPtr& slam_odom){
    //pub odom
    nav_msgs::Odometry slam_odom_temp = *slam_odom;
    slam_odom_temp.header.frame_id = "world";
    slam_odom_pub_.publish(slam_odom_temp);

    //pub path
    geometry_msgs::PoseStamped pose_stamped_temp;           
    pose_stamped_temp.pose=slam_odom_temp.pose.pose;
    pose_stamped_temp.header.stamp= slam_odom_temp.header.stamp;
    pose_stamped_temp.header.frame_id="world";
    slam_odom_path_.header=pose_stamped_temp.header;
    slam_odom_path_.poses.emplace_back(pose_stamped_temp);
    slam_odom_path_pub_.publish(slam_odom_path_);

    std::string pc_path_name_="/media/st/Ramble/0NRSL_data/project/tj/tracker_slam_gt.txt";
    saveTrajectory(pc_path_name_,slam_odom_temp.header.stamp.toSec()-328463.6221,
                   slam_odom_temp.pose.pose.position.x,slam_odom_temp.pose.pose.position.y,slam_odom_temp.pose.pose.position.z,
                   slam_odom_temp.pose.pose.orientation.w,slam_odom_temp.pose.pose.orientation.x,
                   slam_odom_temp.pose.pose.orientation.y,slam_odom_temp.pose.pose.orientation.z,
                   slam_odom_temp.twist.twist.linear.x,slam_odom_temp.twist.twist.linear.y,slam_odom_temp.twist.twist.linear.z);

}

int main(int argc, char **argv){
    if(ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,ros::console::levels::Debug))
    {
        ros::console::notifyLoggerLevelsChanged();
    }

    ros::init(argc, argv, "handle_gps");
    ros::NodeHandle nh;

    ros::Subscriber tracker_gps_odom_sub = nh.subscribe("/novatel/oem7/odom", 10, tracker_gps_odom_CB); 
    ros::Subscriber target_gps_odom_sub = nh.subscribe("/odometry/imu", 10, slam_odom_CB); 

    tracker_odom_pub_ = nh.advertise<nav_msgs::Odometry>("/tracker/odom", 1);
    tracker_odom_path_pub_ = nh.advertise<nav_msgs::Path>("/tracker/path", 10,true);

    slam_odom_pub_ = nh.advertise<nav_msgs::Odometry>("/slam/odom", 1);
    slam_odom_path_pub_ = nh.advertise<nav_msgs::Path>("/slam/path", 10,true);

    ros::spin();

    return 0;
}
